/**
  **************************************************************************
  * @file     mpu_deal.c
  * @brief    mpu_deal program
  **************************************************************************
  *                       Copyright notice & Disclaimer
  *
  * The software Board Support Package (BSP) that is made available to
  * download from Artery official website is the copyrighted work of Artery.
  * Artery authorizes customers to use, copy, and distribute the BSP
  * software and its related documentation for the purpose of design and
  * development in conjunction with Artery microcontrollers. Use of the
  * software is governed by this copyright notice and the following disclaimer.
  *
  * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES,
  * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS,
  * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR
  * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS,
  * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT.
  *
  **************************************************************************
  */

#include "mpu_deal.h"
#include "math.h"

/* the initial position and attitude angles are: 0, 0, 0, corresponding to quaternions: 1, 0, 0, 0 */
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;

/* integral of the error */
float exInt = 0, eyInt = 0, ezInt = 0;

/* half of the sampling period */
#define halfT (MPU_DEAL_TIME / 1000.0 / 2)

/**
  * @brief  mpu6050 data deal(complementary filtering mode).
  * @param  mpu:  mpu: data struct.
  * @retval none.
  */
void mpu_data_deal(mpu_handle_type *mpu)
{
  static float angle_z = 0;
  int16_t x;
  float temp;

  float gx, gy, gz, ax, ay, az;
  float q0temp, q1temp, q2temp, q3temp;
  float norm;
  float vx, vy, vz;
  float ex, ey, ez;

  float q0q0 = q0 * q0;
  float q0q1 = q0 * q1;
  float q0q2 = q0 * q2;
  float q1q1 = q1 * q1;
  float q1q3 = q1 * q3;
  float q2q2 = q2 * q2;
  float q2q3 = q2 * q3;
  float q3q3 = q3 * q3;

  /* unit g */
  ax = mpu->ax * 9.8 / 16384;
  ay = mpu->ay * 9.8 / 16384;
  az = mpu->az * 9.8 / 16384;

  /* unit radians/s */
  gx = mpu->gx * 250 / 57.3 / 32768;
  gy = mpu->gy * 250 / 57.3 / 32768;
  gz = mpu->gz * 250 / 57.3 / 32768;

  /* when the additive is in a free falling state, attitude calculation is not performed */
  if(ax * ay * az==0)
    return;

  norm = sqrt(ax * ax + ay * ay + az * az);
  ax = ax / norm;
  ay = ay / norm;
  az = az / norm;

  vx = 2 * (q1q3 - q0q2);
  vy = 2 * (q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3;

  ex = (ay * vz - az * vy);
  ey = (az * vx - ax * vz);
  ez = (ax * vy - ay * vx);

  exInt = exInt + ex * MPU_Ki;
  eyInt = eyInt + ey * MPU_Ki;
  ezInt = ezInt + ez * MPU_Ki;

  gx = gx + MPU_Kp * ex + exInt;
  gy = gy + MPU_Kp * ey + eyInt;
  gz = gz + MPU_Kp * ez + ezInt;

  q0temp = q0;
  q1temp = q1;
  q2temp = q2;
  q3temp = q3;

  q0 = q0temp + (-q1temp * gx - q2temp * gy - q3temp * gz)*halfT;
  q1 = q1temp + (q0temp * gx + q2temp * gz - q3temp * gy)*halfT;
  q2 = q2temp + (q0temp * gy - q1temp * gz + q3temp * gx)*halfT;
  q3 = q3temp + (q0temp * gz + q1temp * gy - q2temp * gx)*halfT;

  norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 = q0 / norm;
  q1 = q1 / norm;
  q2 = q2 / norm;
  q3 = q3 / norm;

  /* yaw */
#if 1
  angle_z = angle_z + gz * halfT * 2 * 57.3f;

  x = angle_z / 360;

  temp = (angle_z - x * 360);

  if(temp < -180)
    mpu->angle_z = temp + 360;
  else if(temp > 180)
    mpu->angle_z = temp - 360;
  else
    mpu->angle_z = temp;
#else
  mpu->angle_z = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
#endif

  /* pitch */
  if(fabs(mpu->angle_x) < 90.0)
  {
    mpu->angle_y  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
  }
  else
  {
    mpu->angle_y  = (fabs(q0* q2 - q1 * q3)/(q0* q2 - q1 * q3)) *(180.0 - fabs(asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3));
  }

  /* roll */
  mpu->angle_x = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3f;

  mpu->q0 = q0;
  mpu->q1 = q1;
  mpu->q2 = q2;
  mpu->q3 = q3;
}
